﻿using System;
using System.Collections.Generic;
using System.Windows;

using System.Text;
using Emgu.CV;
using Emgu.CV.Structure;
using System.Drawing;
using Domotica.Windows;

namespace Domotica.Gazing
{
    public enum EyeState
    {
        Open,
        Closed,
        ReOpen,
        Unknown
    }

    internal class FeatureTracker
    {
        public FeatureTracker(Image<Gray, byte> faceFrame, Rect rectangle)
        {
            _FaceFrame = faceFrame;

            FindGoodFeaturesToTrack(rectangle);
        }

        private Image<Gray, byte> _FaceFrame;
        private PointF[] _GoodFeaturesToTrack;

        public bool Valid
        {
            get
            {
                return _GoodFeaturesToTrack.Length > 20;
            }
        }

        public double Height
        {
             get
             {
                 const int s = 3;
                 double h = 0;
                 if (_GoodFeaturesToTrack.Length > 3)
                 {
                     PointF c = _GoodFeaturesToTrack[s];
                     for (int i = s; i < _GoodFeaturesToTrack.Length - s; i++)
                     {
                         PointF p = _GoodFeaturesToTrack[i];
                         h += p.Y - c.Y;
                     }
                 }
                 return h;
             }
        }

        private EyeState _EyeStatus;
        public EyeState CheckEyeStatus()
        {
            double h = Height;
            switch (_EyeStatus)
            {
                case EyeState.Open :
                    {
                        if ((h / _StartHeight) < 0.9)
                        {
                            _EyeStatus = EyeState.Closed;
                        }
                        _StartHeight = h;
                        break;
                    }
                case EyeState.Closed:
                    {
                        if ((h / _StartHeight) > 1.1)
                        {
                            _EyeStatus = EyeState.ReOpen;
                        }
                        break;
                    }
            }
            return _EyeStatus;
        }

        private double _StartHeight;

        private void FindGoodFeaturesToTrack(Rect rectangle)
        {
            _FaceFrame.ROI = rectangle.ConvertToRectangle();
            PointF[][] gftt = _FaceFrame.GoodFeaturesToTrack(500, 0.01, 5, 3);
            _FaceFrame.ROI = Rectangle.Empty;

            for (int i = 0; i < gftt[0].Length; i++)
            {
                PointF p = gftt[0][i];
                gftt[0][i] = new PointF(p.X + (float)rectangle.Left, p.Y + (float)rectangle.Top);
            }

            _FaceFrame.FindCornerSubPix(gftt, new System.Drawing.Size(10, 10), new System.Drawing.Size(-1,-1), new MCvTermCriteria(20, 0.03));
            _GoodFeaturesToTrack = gftt[0];
            Array.Sort(_GoodFeaturesToTrack, new PointFComparer());

            _StartHeight = Height;
        }

        public class PointFComparer : IComparer<PointF>
        {
            public int Compare(PointF x, PointF y)
            {
                int c = x.Y.CompareTo(y.Y);
                if (c != 0)
                {
                    return c;
                }
                return x.X.CompareTo(y.X);
            }
        } 
        public static System.Drawing.Point[][] ConvertToPoint(PointF[][] goodFeaturesToTrack)
        {
            System.Drawing.Point[][] gftt = new System.Drawing.Point[goodFeaturesToTrack.Length][];
            for (int i = 0; i < goodFeaturesToTrack.Length; i++)
            {
                System.Drawing.Point[] points = new System.Drawing.Point[goodFeaturesToTrack[i].Length];
                gftt[i] = points;
                for (int j = 0; j < goodFeaturesToTrack[i].Length; j++)
                {
                    System.Drawing.PointF point = goodFeaturesToTrack[i][j];

                    points[j] = new System.Drawing.Point((int)point.X, (int)point.Y);
                }
            }
            return gftt;
        }
        public static System.Drawing.Point[] ConvertToPoint(PointF[] goodFeaturesToTrack)
        {
            System.Drawing.Point[] gftt = new System.Drawing.Point[goodFeaturesToTrack.Length];
            for (int i = 0; i < goodFeaturesToTrack.Length; i++)
            {
                    PointF point = goodFeaturesToTrack[i];
                    gftt[i] = new System.Drawing.Point((int)point.X, (int)point.Y);
            }
            return gftt;
        }

        public bool Track(Image<Gray, byte> frame)
        {
            Image<Gray, byte> faceFrame = frame;
            PointF[] newFeatures;
            byte[] status;
            float[] trackError;
            OpticalFlow.PyrLK(_FaceFrame, faceFrame, _GoodFeaturesToTrack, new System.Drawing.Size(10, 10), 1,
                new MCvTermCriteria(20, 0.03), out newFeatures, out status, out trackError);

            for (int i = 0; i < _GoodFeaturesToTrack.Length; i++)
            {
                if (status[i] == 0 || trackError[i] > 600)
                {
                    return false;
                }
               
            }

            CalculateTrackingPoint();

            _FaceFrame = faceFrame;
            _GoodFeaturesToTrack = newFeatures;
            Array.Sort(_GoodFeaturesToTrack, new PointFComparer());
            return true;
        }

        public void DrawFeatures(Image<Bgr, byte> image)
        {
            System.Drawing.Point[] goodFeaturesToTrackInt = ConvertToPoint(_GoodFeaturesToTrack);

            image.DrawPolyline(goodFeaturesToTrackInt, false, new Bgr(Color.Yellow), 1);
        }

        public void DrawTrackingPoint(Image<Bgr, byte> image)
        {
            image.Draw(new Rectangle(new System.Drawing.Point((int) TrackingPoint.X, (int)TrackingPoint.Y), new System.Drawing.Size(5,5)), new Bgr(Color.Green), 1);
        }

        private System.Windows.Point _TrackingPoint;
        public System.Windows.Point TrackingPoint
        {
            get
            {
                return _TrackingPoint; 
            }
        }

        private void CalculateTrackingPoint()
        {
            const int c = 10;
            if (_GoodFeaturesToTrack.Length < c)
            {
                return;
            }

            float[] x = new float[_GoodFeaturesToTrack.Length];
            float[] y = new float[_GoodFeaturesToTrack.Length];
            for (int i = 0; i < _GoodFeaturesToTrack.Length; i++)
            {
                x[i] = _GoodFeaturesToTrack[i].X;
                y[i] = _GoodFeaturesToTrack[i].Y;
            }
            Array.Sort(x);
            Array.Sort(y);
            float tx = 0;
            float ty = 0;
            int ts = (_GoodFeaturesToTrack.Length / 2) - (c / 2);
            for (int i = 0; i < c; i++)
            {
                tx += x[i + ts];
                ty += y[i + ts];
            }
            _TrackingPoint = new System.Windows.Point(tx / c, ty / c);
        }
    }
}
